/* main.c - Software vector mode program using C isr for MPC56xxB/P/S */
/* Jan 15, 2009 S.Mihalik- Initial version based on previous AN2865 example */
/* Feb 02, 2009 S.Mihalik- Changed to run in RUN0 mode instead of all DRUN mode*/
/* May 22 2009 S. Mihalik- Simplifed by removing unneeded sysclk, PCTL code */
/* Jul 03 2009 S Mihalik - Simplified code */
/* Mar 14 2010 SM - modified initModesAndClock, updated header file */
/* Copyright Freescale Semiconductor, Inc. 2009. All rights reserved. */

//#include "MPC5604B_0M27V_0102.h" /* Use proper include file */

#include "MPC5604B_M07N.h"
#include <math_api.h>
#include <stdio_api.h>
#include <uart.h>

extern IVOR4Handler();
extern uint32_t __IVPR_VALUE; /* Interrupt Vector Prefix vaue from link file*/
extern const vuint32_t IntcIsrVectorTable[];

void TransmitCharacter(uint8_t ch);

#define MAX_LEFT 1200		//maximum stanga pt directie->loveste de sasiu daca-i dau mai mult
#define MAX_RIGHT 1970		//maximum dreapta-> analog mai sus
#define STRAIGHT 1570		// asta-i drept

#define MAX_LEFT_BREAK 1160		//frana maxima->daca punem servo-uri mai zdravene ii vom putea da mai mult
#define MIN_LEFT_BREAK 1000		//servo-ul de pe stga e la 0(sus de tot)
#define MAX_RIGHT_BREAK 1840	//frana maxima pe dreapta
#define MIN_RIGHT_BREAK 2000	//servoul de pe dreapta la 180(sus de tot)

#define BREAK_AMOUNT 12	//cat de mult pune frana de motor
#define ACCELERATION 600 //daca viteza calculata e mai mare decat ACCELERATION atunci se limiteaza la speed+100
#define BOOST 150
#define BREAKING_TIME 250 	//cat timp sunt actionate servo-urile pt frana
#define MCX 450

volatile uint8_t Result[128],Result_binar[128];	/* Read converstion result from ADC input ANS0 */
	//in Result tin datele venite de la camera. Date brute
	//in Result binar tin valori de 0 sau 255 pe care apoi fac prelucrari
volatile int osc[20],amp=1,nr_osc=1;
volatile uint32_t lmc,rmc,min,max,left_margin = 13,right_margin = 114;
volatile uint32_t dly,lly,chw,adcdata,curdata,viraj;
volatile uint32_t lightdata;
volatile uint32_t amount;
volatile int window_size = 13,average,mij[3],speed=600,center;
volatile int half_window_size,angle,counter = 0,pt;
volatile int angle_queue[6][2],valid_result;
volatile uint8_t intensity;
int16_t posr,posl;
uint8_t rx_data[4];

//###################### FLAG AREA ####################################
// FLAGURI START_STOP
uint8_t FLAG_START_STOP = 0;
uint32_t TIME_COUNTER_IRQ = 0;
uint8_t StartStopCounter = 0;

//FLAGURI FRANA
uint8_t FLAG_LEFT_BREAK = 0;
uint8_t FLAG_RIGHT_BREAK = 0;
uint32_t TIME_COUNTER_BREAK = 0;



//#####################################################################



volatile float kp=5.45,kd=4.42,ki=0.010;	//kd=4.32
//float kp=5.45,kd=4.12,ki=0.005;	//kd=4.12 LATEST
//float kp=0.2,kd=0.4,ki=0.05,p;	//*40-> kp = 8; kd = 16; ki = 2;

uint8_t turnLeft = 2;
uint8_t  TransData[10]; /* Transmit string & CR*/

uint32_t Pit1Ctr = 0;   /* Counter for PIT 1 interrupts */
uint32_t SWirq4Ctr = 0;	/* Counter for software interrupt 4 */


void TransmitCharacter(uint8_t ch)
{
	LINFLEX_1.BDRL.B.DATA0 = ch;  			/* write character to transmit buffer */
	while (1 != LINFLEX_1.UARTSR.B.DTF) {}  /* Wait for data transmission completed flag */
	LINFLEX_1.UARTSR.R = 0x0002; 			/* clear the DTF flag and not the other flags */	
}

void TransmitData (char TransData[]) 
{
	uint8_t	j,k;                                 /* Dummy variable */
	k = strlen (TransData);
	for (j=0; j< k; j++) 
	{  /* Loop for character string */

		TransmitCharacter(TransData[j]);  		/* Transmit a byte */		
	}
}

void TransmitNumber(int nr)
{
	int aux = nr;
	if(aux<10)
	{
		TransmitCharacter((aux%10) + 48);	
	}
	if(aux<100 && aux>9)
	{
		TransmitCharacter((aux/10)%10 + 48);
		TransmitCharacter((aux%10) + 48);	
	}
	if(aux>99)
	{
		TransmitCharacter((aux/100)%10 + 48);
		TransmitCharacter((aux/10)%10 + 48);
		TransmitCharacter((aux%10) + 48);	
	}
}

void printserialhex(uint16_t innum) {
  uint16_t j1,in;
  uint8_t p1,p2;
  in = innum;
   
  j1 = (in & 0x0f);
  if (j1 > 9) p1 = (uint8_t)(j1 + 0x41 - 10);
  else p1 = (uint8_t)(j1 +0x30);
  j1 = (in & 0xf0) >> 4;
  if (j1 > 9) p2 = (uint8_t)(j1 +0x41 - 10);
  else p2 = (uint8_t)(j1 +0x30);
  TransmitCharacter(p2);
  TransmitCharacter(p1);  
}

void printserialsingned(uint16_t innum) {
  uint16_t j1,k1,l1,m1,in;
  uint8_t p1,p2,p3,p4,p5;
 
  if(innum < 0x8000) {
    in = innum;
  	TransmitCharacter('+');    
  } 
  else {
    in = (uint16_t)(~innum);
    //in = 0x7fff - in;
    TransmitCharacter('-');     
  }
  
  j1 = (in / 10);
  p1 = (uint8_t)(in - j1*10 +0x30);
  k1 = (j1 / 10);
  p2 = (uint8_t)(j1 - k1*10 +0x30);
  l1 = (k1 / 10);
  p3 = (uint8_t)(k1 - l1*10 +0x30);
  m1 = (l1 / 10);
  p4 = (uint8_t)(l1 - m1*10 +0x30);
  p5 = (uint8_t)m1 +0x30;
  TransmitCharacter(p5);
  TransmitCharacter(p4);
  TransmitCharacter(p3);
  TransmitCharacter(p2);
  TransmitCharacter(p1);  
  TransmitCharacter(0x09);
}

void printlistall(void) {
   TransmitCharacter(0x0a);   
   TransmitCharacter(0x0d);  
   for(pt=0;pt<120;pt++){
      //pt++;
      //pt++;
      printserialhex(Result[pt]);
      //printserial(list[pt]);
   }
   TransmitCharacter(0x0a);   
   TransmitCharacter(0x0d);   
}

void PrintResult(int start,int stop)
{
	TransmitData("**MASURATOARE**\n\r");
	for(pt = 14;pt<114;pt++)
	{
		TransmitNumber(Result_binar[pt]);
		TransmitData(" ");
	}
		
	TransmitData("\n\r");
}

void init_LinFLEX_0_UART (void) 
{	

	/* enter INIT mode */
	LINFLEX_1.LINCR1.R = 0x0081; 		/* SLEEP=0, INIT=1 */
	
	/* wait for the INIT mode */
	while (0x1000 != (LINFLEX_1.LINSR.R & 0xF000)) {}
		
	/* configure pads */
	//SIU.PCR[18].R = 0x0604;     		/* Configure pad PB2 for AF1 func: LIN0TX */
	//SIU.PCR[19].R = 0x0100;     		/* Configure pad PB3 for LIN0RX */	
	
	SIU.PCR[38].R = 0x0604;     		/* Configure pad PC6 for AF1 func: LIN1TX */
	SIU.PCR[39].R = 0x0100;     		/* Configure pad PC7 for LIN1RX */	
	
	/* configure for UART mode */
	LINFLEX_1.UARTCR.R = 0x0001; 		/* set the UART bit first to be able to write the other bits */
	LINFLEX_1.UARTCR.R = 0x0033; 		/* 8bit data, no parity, Tx and Rx enabled, UART mode */
								 		/* Transmit buffer size = 1 (TDFL = 0 */
								 		/* Receive buffer size = 1 (RDFL = 0) */
	
	/* configure baudrate 115200 */
	/* assuming 64 MHz peripheral set 1 clock */		
	LINFLEX_1.LINFBRR.R = 12;
	LINFLEX_1.LINIBRR.R = 34;
		
	/* enter NORMAL mode */
	LINFLEX_1.LINCR1.R = 0x0080; /* INIT=0 */	
}

/* This functions polls UART receive buffer. when it is full, it moves received data from the buffer to the memory */
uint8_t ReadData (void)
{
	uint8_t ch;
	/* wait for DRF */
	while (1 != LINFLEX_1.UARTSR.B.DRF) {}  /* Wait for data reception completed flag */
		
	/* wait for RMB */
	while (1 != LINFLEX_1.UARTSR.B.RMB) {}  /* Wait for Release Message Buffer */
	
	/* get the data */
	ch = (uint8_t)LINFLEX_1.BDRM.B.DATA4;
		
	/* clear the DRF and RMB flags by writing 1 to them */
	LINFLEX_1.UARTSR.R = 0x0204;
	
	return ch;
	
}
	/*	Aici fac initializarea referitoare la modurile de rulare
	 *	Ne intereseaza doar DRUN, RUN0, SAFE si RESET-ul
	 *	Setam sursa de clock: extern la 8Mhz din care apoi fac 64MHz
	 *	Aleg ca mod de functionare RUN0
	 *	Fac setup-ul: adaug perifericele pt modul de rulare RUN0(cu ME.PCTL ala)
	 *	Adaug ADC-ul, CTUL, Seriala, SIUL, EMIOS, PIT,RTI
	 *	Fac apo tranzitia in modul RUN0 si ma asigur ca s-a efectuat cu succes
	 */
void initModesAndClock(void) {
	ME.MER.R = 0x0000001D;          	/* Enable DRUN, RUN0, SAFE, RESET modes */
	                              		/* Initialize PLL before turning it on: */
										/* Use 1 of the next 2 lines depending on crystal frequency: */
	CGM.FMPLL_CR.R = 0x02400100;    	/* 8 MHz xtal: Set PLL0 to 64 MHz */   
	/*CGM.FMPLL_R = 0x12400100;*/     	/* 40 MHz xtal: Set PLL0 to 64 MHz */   
	ME.RUN[0].R = 0x001F0074;       	/* RUN0 cfg: 16MHzIRCON,OSC0ON,PLL0ON,syclk=PLL */
	
	//ME.RUNPC[0].R = 0x00000010; 	  	/* Peri. Cfg. 0 settings: only run in RUN0 mode */
   										/* Use the next lines as needed for MPC56xxB/S: */  	    	
	//ME.PCTL[48].R = 0x0000;         	/* MPC56xxB LINFlex0: select ME.RUNPC[0] */	
	//ME.PCTL[68].R = 0x0000;         	/* MPC56xxB/S SIUL:  select ME.RUNPC[0] */	
	
	ME.RUNPC[1].R = 0x00000010;     	/* Peri. Cfg. 1 settings: only run in RUN0 mode */
	ME.PCTL[32].R = 0x01;       		/* MPC56xxB ADC 0: select ME.RUNPC[1] */
  	ME.PCTL[57].R = 0x01;       		/* MPC56xxB CTUL: select ME.RUNPC[1] */
  	//ME.PCTL[48].R = 0x01;           	/* MPC56xxB/P/S LINFlex 0: select ME.RUNPC[1] */
  	ME.PCTL[49].R = 0x01;				/* MPC56xxB/P/S LINFlex 1: select ME.RUNPC[1] */
	ME.PCTL[68].R = 0x01;           	/* MPC56xxB/S SIUL:  select ME.RUNPC[1] */
	ME.PCTL[72].R = 0x01;           	/* MPC56xxB/S EMIOS 0:  select ME.RUNPC[1] */
	ME.PCTL[92].R = 0x01;           /* PIT, RTI: select ME_RUN_PC[1] *
	                              		/* Mode Transition to enter RUN0 mode: */
	ME.MCTL.R = 0x40005AF0;         	/* Enter RUN0 Mode & Key */
	ME.MCTL.R = 0x4000A50F;         	/* Enter RUN0 Mode & Inverted Key */  
	while (ME.GS.B.S_MTRANS) {}     	/* Wait for mode transition to complete */    
	                          			/* Note: could wait here using timer and/or I_TC IRQ */
	while(ME.GS.B.S_CURRENTMODE != 4) {}/* Verify RUN0 is the current mode */
	
	while (ME.IS.B.I_MTC != 1) {}    /* Wait for mode transition to complete */    
	ME.IS.R = 0x00000001;           /* Clear Transition flag */ 
}

/*
 *	Initializez clock-ul pt periferice: va fi sysclk(64Mhz) cu prescaler 1
 */
void initPeriClkGen(void) {
	CGM.SC_DC[0].R = 0x80; 				/* MPC56xxB/S: Enable peri set 1 sysclk divided by 1 */
  	CGM.SC_DC[2].R = 0x80;         		/* MPC56xxB: Enable peri set 3 sysclk divided by 1*/
}

void disableWatchdog(void) {
	SWT.SR.R = 0x0000c520;     			/* Write keys to clear soft lock bit */
  	SWT.SR.R = 0x0000d928; 
  	SWT.CR.R = 0x8000010A;     			/* Clear watchdog enable (WEN) */
}

/*
 * EMIOS-u e pt a scoate PWM, OPWM, SAIC, ADC si alte functii pe pini
 * tot cu EMIOS-ul poti implementa timere si da intreruperi
 * ANP sunt canalele de ADC de precizie (sunt 3 tipuri de canale: precizie, generale si multiplexate)
 * ANP0 e pt camera
 * ANP1,2,4 nu mai stiu pt ce le-am folosit:d
 */

void initPads (void) {
	SIU.PCR[2].R = 0x0503;           	/* MPC56xxB: Initialize PA[2] as eMIOS[2] input */
	SIU.PCR[3].R = 0x0600;           	/* MPC56xxB: Initialize PA[3] as eMIOS[3] output */
	SIU.PCR[20].R = 0x2000;          	/* MPC56xxB: Initialize PB[4] as ANP0 */
	SIU.PCR[21].R = 0x2000;          	/* MPC56xxB: Initialize PB[5] as ANP1 */
	SIU.PCR[22].R = 0x2000;          	/* MPC56xxB: Initialize PB[6] as ANP2 */
	SIU.PCR[48].R = 0x2000;          	/* MPC56xxB: Initialize PD[0] as ANP4 */
}

void initADC(void) {
	//ADC.MCR.R = 0x20020000;         	/* Initialize ADC scan mode*/
	ADC.MCR.R = 0x00000000;         	/* Initialize ADC one shot mode*/
	ADC.NCMR[0].R = 0x00000017;      	/* Select ANP1:2 inputs for normal conversion */
	ADC.CTR[0].R = 0x00008606;       	/* Conversion times for 32MHz ADClock */
}

/*
 * Aici configurez Cross Triggering Unit
 * asta genereaza eventuri corelate cu timerele de pe EMIOS
 */
void initCTU(void) {
  	CTU.EVTCFGR[0].R = 0x00008001;  	 /* Config event on eMIOS Ch 2 to trig ANP[0] */
}

/*
 * Initializare pt EMIOS_0: sunt 2 canale de EMOIS(EMIOS0 si EMIOS1)
 * Trebuie sa-i setez clock-ul pe magistrala: e de 1MHz
 * Opresc EMIOS-ul cand sunt in debug
 * Pun ca 1MHz sa fie global time base
 */
void initEMIOS_0(void) {  
	EMIOS_0.MCR.B.GPRE= 63;   			/* Divide 64 MHz sysclk by 63+1 = 64 for 1MHz eMIOS clk*/
	EMIOS_0.MCR.B.GPREN = 1;			/* Enable eMIOS clock */
	EMIOS_0.MCR.B.GTBE = 1;  			/* Enable global time base */
	EMIOS_0.MCR.B.FRZ = 1;    			/* Enable stopping channels when in debug mode */
}

void initEMIOS_0ch3(void) {
	EMIOS_0.CH[3].CADR.R = 250;      	/* Ch 3: Match "A" is 250 */
	EMIOS_0.CH[3].CBDR.R = 500;      	/* Ch 3: Match "B" is 500 */
	EMIOS_0.CH[3].CCR.R= 0x000000E0; 	/* Ch 3: Mode is OPWMB, time base = ch 23 */
	EMIOS_0.CH[2].CCR.R= 0x01020082; 	/* Ch 2: Mode is SAIC, time base = ch 23 */
}

/*
 * Iau canalul CH0 pt EMIOS0 si il folosesc ca sa dea clock la CH1-CH7
 * Cu asta spun practic perioada la PWM si atat.
 * Cu restul canalelor vin si spun cand apare rising edge-ul si falling-edge-ul
 * cu alte cuvinte aici ii spun base time-u si dupa aia pot spune la restul canalelor sa foloseasca 
 * clock-ul asta.
 */
void initEMIOS_0ch0(void) {        		/* EMIOS 0 CH 0: Modulus Up Counter */
	EMIOS_0.CH[0].CADR.R = 19999;   	/* Period will be 19999+1 = 20000 clocks (20 msec)*/
	EMIOS_0.CH[0].CCR.B.MODE = 0x50; 	/* Modulus Counter Buffered (MCB) */
	EMIOS_0.CH[0].CCR.B.BSL = 0x3;   	/* Use internal counter */
	EMIOS_0.CH[0].CCR.B.UCPRE=0;     	/* Set channel prescaler to divide by 1 */
	EMIOS_0.CH[0].CCR.B.UCPEN = 1;   	/* Enable prescaler; uses default divide by 1  !!!!!!!!!!!!!!!!!!!!pot folosi prescaler!!!!!!*/
	EMIOS_0.CH[0].CCR.B.FREN = 1;   	/* Freeze channel counting when in debug mode*/
}

/*
 * asta e counter bus A pe care il pot folosi toate canalele de EMIOS0
 */
void initEMIOS_0ch23(void) {        	/* EMIOS 0 CH 23: Modulus Up Counter */
	EMIOS_0.CH[23].CADR.R = 999;      	/* Period will be 999+1 = 1000 clocks (1 msec)*/
	EMIOS_0.CH[23].CCR.B.MODE = 0x50; 	/* Modulus Counter Buffered (MCB) */
	EMIOS_0.CH[23].CCR.B.BSL = 0x3;   	/* Use internal counter */
	EMIOS_0.CH[23].CCR.B.UCPRE=0;     	/* Set channel prescaler to divide by 1 */
	EMIOS_0.CH[23].CCR.B.UCPEN = 1;   	/* Enable prescaler; uses default divide by 1*/
	EMIOS_0.CH[23].CCR.B.FREN = 1;   	/* Freeze channel counting when in debug mode*/
}
/*
 * asta e servoul pt directie
 * perioada totala e de 20ms->numara pana la 20000
 * ca sa am factor de umplere de 2ms trebuie sa numar pana la 2000
 * CADR.R si CBDR.R sunt un fel de OC1A si OC1B de pe Atmeluri
 * de asemenea fiecare canal de EMIOS trebuie sa-l atribui pin-ului care are respectiva functionalitate
 * Same ca la Atmel-uri: daca vrei output pe pin-ul respectiv sau vrei doar timer-ul pt software
 */
void initEMIOS_0ch4(void) {        		/* EMIOS 0 CH 4: Output Pulse Width Modulation*/
	EMIOS_0.CH[4].CADR.R = 0;     		/* Leading edge when channel counter bus=0*/
	EMIOS_0.CH[4].CBDR.R = 1500;      	/* Trailing edge when channel counter bus=1400 Middle, 1650 Right Max, 1150 Left Max*/
	EMIOS_0.CH[4].CCR.B.BSL = 0x01;  	/* Use counter bus B */
	EMIOS_0.CH[4].CCR.B.EDPOL = 1;  	/* Polarity-leading edge sets output */
	EMIOS_0.CH[4].CCR.B.MODE = 0x60; 	/* Mode is OPWM Buffered */
	SIU.PCR[28].R = 0x0600;           	/* MPC56xxS: Assign EMIOS_0 ch 6 to pad */
}
//asta-i un motor de tractiune
void initEMIOS_0ch6(void) {        		/* EMIOS 0 CH 6: Output Pulse Width Modulation*/
	EMIOS_0.CH[6].CADR.R = 0;     	/* Leading edge when channel counter bus=0*/
	EMIOS_0.CH[6].CBDR.R = 0;     	/* Trailing edge when channel counter bus=500*/
	EMIOS_0.CH[6].CCR.B.BSL = 0x0;  	/* Use counter bus A (default) */
	EMIOS_0.CH[6].CCR.B.EDPOL = 1;  	/* Polarity-leading edge sets output */
	EMIOS_0.CH[6].CCR.B.MODE = 0x60; 	/* Mode is OPWM Buffered */
	SIU.PCR[30].R = 0x0600;           	/* MPC56xxS: Assign EMIOS_0 ch 6 to pad */
}
//asta-i celalalt
void initEMIOS_0ch7(void) {        		/* EMIOS 0 CH 7: Output Pulse Width Modulation*/
	EMIOS_0.CH[7].CADR.R = 0;    		/* Leading edge when channel counter bus=0*/
	EMIOS_0.CH[7].CBDR.R = 0;     	/* Trailing edge when channel's counter bus=999*/
	EMIOS_0.CH[7].CCR.B.BSL = 0x0; 		/* Use counter bus A (default) */
	EMIOS_0.CH[7].CCR.B.EDPOL = 1; 		/* Polarity-leading edge sets output*/
	EMIOS_0.CH[7].CCR.B.MODE = 0x60; 	/* Mode is OPWM Buffered */
	SIU.PCR[31].R = 0x0600;           	/* MPC56xxS: Assign EMIOS_0 ch 7 to pad */
}
//asta-i pt led-uri
void initEMIOS_0ch9(void) {        		/* EMIOS 0 CH 9: Output Pulse Width Modulation*/
	EMIOS_0.CH[9].CADR.R = 0;     	/* Leading edge when channel counter bus=0*/
	EMIOS_0.CH[9].CBDR.R = 0;     	/* Trailing edge when channel counter bus=500*/
	EMIOS_0.CH[9].CCR.B.BSL = 0x0;  	/* Use counter bus A (default) */
	EMIOS_0.CH[9].CCR.B.EDPOL = 1;  	/* Polarity-leading edge sets output */
	EMIOS_0.CH[9].CCR.B.MODE = 0x60; 	/* Mode is OPWM Buffered */
	SIU.PCR[9].R = 0x0600;           	/* MPC56xxS: Assign EMIOS_0 ch 1 to pad */
}
//configurare PWM pt servo-uri frana

void initEMIOS_0ch8(void) {        		/* EMIOS 0 CH 8: Modulus Up Counter */
	EMIOS_0.CH[8].CADR.R = 19999;   	/* Period will be 19999+1 = 20000 clocks (20 msec)*/
	EMIOS_0.CH[8].CCR.B.MODE = 0x50; 	/* Modulus Counter Buffered (MCB) */
	EMIOS_0.CH[8].CCR.B.BSL = 0x3;   	/* Use internal counter */
	EMIOS_0.CH[8].CCR.B.UCPRE=0;     	/* Set channel prescaler to divide by 1 */
	EMIOS_0.CH[8].CCR.B.UCPEN = 1;   	/* Enable prescaler; uses default divide by 1*/
	EMIOS_0.CH[8].CCR.B.FREN = 1;   	/* Freeze channel counting when in debug mode*/}

void initEMIOS_0ch10(void) {        		/* EMIOS 0 CH 10: Output Pulse Width Modulation*/
	EMIOS_0.CH[10].CADR.R = 0;     		/* Leading edge when channel counter bus=0*/
	EMIOS_0.CH[10].CBDR.R = 0;      	/* Trailing edge when channel counter bus=1400 Middle, 1650 Right Max, 1150 Left Max*/
	EMIOS_0.CH[10].CCR.B.BSL = 0x01;  	/* Use counter bus C */
	EMIOS_0.CH[10].CCR.B.EDPOL = 1;  	/* Polarity-leading edge sets output */
	EMIOS_0.CH[10].CCR.B.MODE = 0x60; 	/* Mode is OPWM Buffered */
	SIU.PCR[10].R = 0x0600;           	/* MPC56xxS: Assign EMIOS_0 ch 10 to pad */
}

void initEMIOS_0ch11(void) {        		/* EMIOS 0 CH 11: Output Pulse Width Modulation*/
	EMIOS_0.CH[11].CADR.R = 0;     		/* Leading edge when channel counter bus=0*/
	EMIOS_0.CH[11].CBDR.R = 0;      	/* Trailing edge when channel counter bus=1400 Middle, 1650 Right Max, 1150 Left Max*/
	EMIOS_0.CH[11].CCR.B.BSL = 0x01;  	/* Use counter bus C */
	EMIOS_0.CH[11].CCR.B.EDPOL = 1;  	/* Polarity-leading edge sets output */
	EMIOS_0.CH[11].CCR.B.MODE = 0x60; 	/* Mode is OPWM Buffered */
	SIU.PCR[11].R = 0x0600;           	/* MPC56xxS: Assign EMIOS_0 ch 11 to pad */
}

void Delay(void){
  //for(dly=0;dly<250;dly++)
  for(dly=0;dly<30;dly++)
  {
  }
}

void Delaylong(void){
  for(dly=0;dly<15000;dly++)
  {
  }
}

void Delaylonglong(void){
  for(lly=0;lly<1;lly++) Delaylong();
}

void Delaycamera(void){
  Delaylong();
  //Delaylong();
  //Delaylong();
  //Delaylong();
 //	Delaylong();
  //Delaylong();
  //Delaylong();
  //Delaylong();
  //Delaylong();
  //Delaylong();
}

void Delaywait(void){
  for(lly=0;lly<500;lly++) Delaylong();
}

void Delayled(void){
  for(lly=0;lly<500;lly++) Delaylong();
}

void LED(void)
{
	SIU.PCR[68].R = 0x0200;				/* Program the drive enable pin of LED1 (PE4) as output*/
	SIU.PCR[69].R = 0x0200;				/* Program the drive enable pin of LED2 (PE5) as output*/
	SIU.PCR[70].R = 0x0200;				/* Program the drive enable pin of LED3 (PE6) as output*/
	SIU.PCR[71].R = 0x0200;				/* Program the drive enable pin of LED4 (PE7) as output*/
	//TransmitData("****Led Test****\n\r");
	//TransmitData("All Led ON\n\r");
	Delayled();
	SIU.PGPDO[2].R |= 0x0f000000;		/* Disable LEDs*/
	SIU.PGPDO[2].R &= 0x07000000;		/* Enable LED1*/
	//TransmitData("Led 1 ON\n\r");
	Delayled();
	SIU.PGPDO[2].R |= 0x08000000;		/* Disable LED1*/
	SIU.PGPDO[2].R &= 0x0b000000;		/* Enable LED2*/
	//TransmitData("Led 2 ON\n\r");
	Delayled();
	SIU.PGPDO[2].R |= 0x04000000;		/* Disable LED2*/
	SIU.PGPDO[2].R &= 0x0d000000;		/* Enable LED3*/
	//TransmitData("Led 3 ON\n\r");
	Delayled();
	SIU.PGPDO[2].R |= 0x02000000;		/* Disable LED3*/
	SIU.PGPDO[2].R &= 0x0e000000;		/* Enable LED4*/
	//TransmitData("Led 4 ON\n\r");
	Delayled();
	SIU.PGPDO[2].R |= 0x01000000;		/* Disable LED4*/
}


void SERVO(void)
{
	TransmitData("****Steering Servo Test****\n\r");
	EMIOS_0.CH[4].CBDR.R = 1500;      	/* 1500 Middle */
	TransmitData("Middle\n\r");
	Delaywait();
	EMIOS_0.CH[4].CBDR.R = 1750;        /* 1750 Right Max,*/
	TransmitData("Right\n\r");
	Delaywait();
	EMIOS_0.CH[4].CBDR.R = 1250;        /* 1250 Left Max*/
	TransmitData("Left\n\r");
	Delaywait();
	EMIOS_0.CH[4].CBDR.R = 1500;      	/* 1500 Middle */
}

void MOTOR_LEFT(void)
{
	//TransmitData("****Left Drive Motor Test****\n\r");
	SIU.PCR[16].R = 0x0200;				/* Program the drive enable pin of Left Motor as output*/
	SIU.PGPDO[0].R = 0x00008000;		/* Enable Left the motors */
	Delaywait();
	SIU.PGPDO[0].R = 0x00000000;		/* Disable Left the motors */
}

void MOTOR_RIGHT(void)
{
	//TransmitData("****Right Drive Motor Test****\n\r");
	SIU.PCR[17].R = 0x0200;				/* Program the drive enable pin of Right Motor as output*/
	SIU.PGPDO[0].R = 0x00004000;		/* Enable Right the motors */
	Delaywait();
	SIU.PGPDO[0].R = 0x00000000;		/* Disable Right the motors */
}

/* fac citire cu camera
 * Cand incep citirea trebuie sa-i dau un puls pe SI( e acel start read)
 * Dau clock pt fiecare pixel pe care-l iau: e un shift register pe camera care scuipa cate un pixel
 * pt fiecare clock pe care i-l dau
 * SIU.PGPDO sunt pt citire/scriere paralela a pinilor de GPIO 
 */
void CAMERA(void)
{
	volatile int aux,i;
	//TransmitData("****Line Sensor Read****\n\r");
	SIU.PCR[27].R = 0x0200;				/* Program the Sensor read start pin as output*/
	SIU.PCR[29].R = 0x0200;				/* Program the Sensor Clock pin as output*/
	SIU.PGPDO[0].R &= ~0x00000014;		/* All port line low */
	SIU.PGPDO[0].R |= 0x00000010;		/* Sensor read start High */
	Delay();
	SIU.PGPDO[0].R |= 0x00000004;		/* Sensor Clock High */
	Delay();
	SIU.PGPDO[0].R &= ~0x00000010;		/* Sensor read start Low */ 
	Delay();
	SIU.PGPDO[0].R &= ~0x00000004;		/* Sensor Clock Low */
	//Delay();
	min = 255;
	max = 0;
	
	for (i=0;i<128;i++)
	{
		Delay();
		SIU.PGPDO[0].R |= 0x00000004;	/* Sensor Clock High */
		ADC.MCR.B.NSTART=1;     		/* Trigger normal conversions for ADC0 */
		while (ADC.MCR.B.NSTART == 1) {};
		adcdata = ADC.CDR[0].B.CDATA;
		Delay();
		SIU.PGPDO[0].R &= ~0x00000004;	/* Sensor Clock Low */
		aux = (uint8_t)(adcdata >> 2);
		Result[i] = aux;
		if(i>=left_margin && i<=right_margin)
		{
			if(aux<min) min = aux;	//determinare minimul inregistrat in citire
			if(aux>max)	max = aux;	//determinare maximul citit	
		}
	}
	counter++;
	Delaycamera();
}

void RIGHT_MOTOR_CURRENT(void)
{
	int i;//TransmitData("****Right Motor Current****\n\r");
	//SIU.PGPDO[0].R = 0x00004000;			/* Enable Right the motors */
	//Delaywait();
	//for (i=0;i <10;i++)
	//{
		ADC.MCR.B.NSTART=1;     			/* Trigger normal conversions for ADC0 */
		while (ADC.MSR.B.NSTART == 1) {};
		rmc = ADC.CDR[2].B.CDATA;
	//	printserialsingned(curdata);		
	//}
	//SIU.PGPDO[0].R = 0x00000000;		/* Disable Right the motors */
}

void LEFT_MOTOR_CURRENT(void)
{
	int i;
	//TransmitData("****Left Motor Current****\n\r");
	//SIU.PGPDO[0].R = 0x00008000;			/* Enable Right the motors */
	//Delaywait();
	//for (i=0;i <10;i++)
	//{
		ADC.MCR.B.NSTART=1;     			/* Trigger normal conversions for ADC0 */
		while (ADC.MSR.B.NSTART == 1) {};
		lmc = ADC.CDR[1].B.CDATA;
	//	printserialsingned(curdata);		
	//}
	//SIU.PGPDO[0].R = 0x00000000;		/* Disable Right the motors */
}

/*
 * Aici aplic KNN (K nearest neighbours) pt a filtra eventualele zgomote
 * Compun vectorul binar cu 0 si 255 pt a prelucra mai departe
 * aici vezi ca poti optimiza codul sa mearga mai repede daca vrei(nu arde ca dupaia punem delay-uri:p)
 */
void WINDOW_NOISE_REDUCTION()
{
	volatile int i,j,sum;
	volatile int treshold;
	average = (max+min)/2;
	if(average < 30)
		treshold = 255;
	else
		treshold =average-max*10/100;
	for(i = left_margin;i<right_margin;i++)
	{
		sum=0;
		for(j=i-half_window_size;j<i+half_window_size;j++)
		{
			if(Result[j]>= treshold)
				sum++;
		}
		if(sum>=half_window_size+1)
			Result_binar[i] = 255;
		else
			Result_binar[i] = 0;
	}
}
/*
 * Aici determin mijlocul liniei
 * Sunt 3 mijlocuri: design-ul a fost facut pt detectia liniei de start/stop(atunci am fi avut 3 zone negre deci 3 mijloace)
 * Asta putem folosi pt cand apare un obstacol care-i putin distantat de linie(exista spatiu alb intre
 * linie si obstacol)
 */
void GETMIJ(volatile uint8_t vect[128])
{
	volatile int w=left_margin+1,c=0;
	mij[0]=0;
	mij[1]=0;
	mij[2]=0;
	if(min<max*75/100)
	{
		
		while(w<right_margin)
		{
			if(vect[w]==255)
			{
				while(vect[w]==255 && w<right_margin)
				{
					w++;
				}
			}
				
			if(w>=right_margin)
			{
				break; 
			}
			
			if(vect[w]==0)
			{
				mij[c]=w;
				while(vect[w]==0 && w<right_margin)
				{
					w++;
				}
				if(w-mij[c]>=5)
				{
					mij[c]=(mij[c]+w)/2;
					c++;
				}
			}
		}
	}
}

// aici stabilesc care-i centrul dintre alea 3 mijloace	
int GETCENTER()
{
	if(mij[0]!=0 && mij[1]==0 && mij[2]==0)
		return mij[0];
	if(mij[0]>0 && mij[1]>0 && mij[2]>0)
		return mij[1];
	if(mij[0]>0 && mij[1]>0 && mij[2] == 0)
		return(mij[0]+mij[1])/2;
	return -1;
}

void SETSPEED(int vitezaL, int vitezaR)
{
	EMIOS_0.CH[6].CBDR.R = vitezaL;
	EMIOS_0.CH[7].CBDR.R = vitezaR;
}

void ENABLEMOTORS(void)
{
	SIU.PCR[16].R = 0x0200;				/* Program the drive enable pin of Left Motor as output*/
	SIU.PCR[17].R = 0x0200;				/* Program the drive enable pin of Right Motor as output*/
	SIU.PGPDO[0].R = 0x0000C000;		/* Enable both motors ENA, ENB*/
}

void STOP(void)
{
	EMIOS_0.CH[6].CBDR.R = 0;
	EMIOS_0.CH[7].CBDR.R = 0;	
	//SIU.PGPDO[0].R = 0x00000000;		//Disable motors
}

void LED_intensity(int pwr)
{
	EMIOS_0.CH[9].CBDR.R = pwr;
}

void TURNANGLE(int turn)
{
	EMIOS_0.CH[4].CBDR.R = turn;
}

/*
 * acum am config urmatoare:
 * BUT1->LED1 : speed
 * BUT2->LED2 : Kp
 * BUT3->LED3 : Ki
 * BUT4->LED4 : Kd
 * putem insa pune pe ele ce vrem noi
 */
void ADJUSTMENTS()
{
	int flag1=1,flag2=1,flag3=1,flag4=1,q;
	
	SIU.PCR[68].R = 0x0200;				/* Program the drive enable pin of LED1 (PE4) as output*/
	SIU.PCR[69].R = 0x0200;				/* Program the drive enable pin of LED2 (PE5) as output*/
	SIU.PCR[70].R = 0x0200;				/* Program the drive enable pin of LED3 (PE6) as output*/
	SIU.PCR[71].R = 0x0200;				/* Program the drive enable pin of LED4 (PE7) as output*/
	//Delayled();
	SIU.PGPDO[2].R |= 0x0f000000;		/* Disable LEDs*/

	if((SIU.PGPDI[2].R & 0x80000000) == 0)
	{
		if(flag1)
		{
			speed+=5;	//switch 1 pressed
			SIU.PGPDO[2].R &= 0x07000000;		/* Enable LED1*/
			for(q=0;q<10;q++)
				Delaycamera();
			flag1=0;		
		}
		
	}
	else
	{
		SIU.PGPDO[2].R |= 0x08000000;		/* Disable LED1*/
		flag1=1;
	}
	if((SIU.PGPDI[2].R & 0x40000000) == 0)
	{
		if(flag2)
		{
			kp+=0.01;//coef_franare+=1;//kp+=1;	//switch 2 pressed
			SIU.PGPDO[2].R &= 0x0b000000;		/* Enable LED2*/
			for(q=0;q<10;q++)
				Delaycamera();
			flag2=0;
			/*TransmitData("kp: ");
			TransmitNumber(kp);	
			TransmitData("\n\r");*/
		}	
	}
	else
	{
		SIU.PGPDO[2].R |= 0x04000000;		/* Disable LED2*/
		flag2=1;
	}
	if((SIU.PGPDI[2].R & 0x20000000) == 0)
	{
		if(flag3)
		{
			ki+=0.001;	//switch 3 pressed
			SIU.PGPDO[2].R &= 0x0d000000;		/* Enable LED3*/
			for(q=0;q<10;q++)
				Delaycamera();
			flag3=0;	
			/*TransmitData("ki: ");
			TransmitNumber(ki);
			TransmitData("\n\r");	*/
		}	
	}
	else
	{
		SIU.PGPDO[2].R |= 0x02000000;		/* Disable LED3*/
		flag3=1;
	}
	if((SIU.PGPDI[2].R & 0x10000000) == 0)
	{
		if(flag4)
		{
			kd+=0.01;	//switch 4 pressed
			SIU.PGPDO[2].R &= 0x0e000000;		/* Enable LED4*/
			for(q=0;q<10;q++)
				Delaycamera();
			flag4=0;	
			/*TransmitData("kd: ");
			TransmitNumber(kd);	
			TransmitData("\n\r");*/
		}	
	}
	else
	{
		flag4=1;
		SIU.PGPDO[2].R |= 0x01000000;		/* Disable LED4*/
	}
}
/* Shiftez stanga o matrice2x3 in care tin un istoric al deciziilor 
 * si pe baza caruia validez citirea curenta
 */
void Deplasare_Stanga()
{
	int q;
	for(q=0;q<2;q++)
	{
		angle_queue[q][0] = angle_queue[q+1][0];
		angle_queue[q][1] = angle_queue[q+1][1];
	}
	angle_queue[2][0]=0;
	angle_queue[2][1]=0;
}

/*void Amplitude()
{
	volatile int minim = 255,maxim = -255;
	int p,aux = 0;
	nr_osc = 1;
	for(p=0;p<19;p++)
	{
		if(osc[p]<minim) minim = osc[p];
		if(osc[p]>maxim) maxim = osc[p];
		if((osc[p]*osc[p+1])<0)
			nr_osc++;
	}
	amp = maxim-minim+1;
	aux = speed*45/(amp*nr_osc);
	if(aux>350)
		SETSPEED(speed+100);
	else SETSPEED(aux);//nr_osc = nr;
}*/

void LeftBreak(int value)
{
	EMIOS_0.CH[10].CBDR.R = value;
	EMIOS_0.CH[6].CBDR.R = 0;	//PWM LEFT MOTOR
}

void RightBreak(int value)
{
	EMIOS_0.CH[11].CBDR.R = value;
	EMIOS_0.CH[7].CBDR.R = 0;	//PWM RIGHT MOTOR			
}

/*void Break()
{
	volatile int ave = 0;
	int i =0;
	for(i=0;i<19;i++)
		ave +=osc[i];
	if(ave/20<osc[19]-20)
	{
		LeftBreak(1150);
		EMIOS_0.CH[6].CBDR.R = 0;
	}
	else
		LeftBreak(1000);
	
	if(ave/20>osc[19]+20)
	{
		RightBreak(1850);
		
	}
	else
		RightBreak(2000);
}*/

/*
 * Calculeaza amplitudinea si numarul oscilatiilor si calculeaza 
 * pe baza lor viteza de deplasare(cata frana trebuie sa puna)
 */
void Amplitude()
{
	volatile int minim = 255,maxim = -255;
	int p,aux = 0,ave=0;
	nr_osc = 1;
	for(p=0;p<19;p++)
	{
		if(osc[p]<minim) minim = osc[p];
		if(osc[p]>maxim) maxim = osc[p];
		if((osc[p]*osc[p+1])<0)
			nr_osc++;
		ave+=osc[p];
	}
	amp = maxim-minim+1;
	aux = speed*BREAK_AMOUNT/(amp*nr_osc);
	if(aux>ACCELERATION)
		// cazut cand turnLeft=2 nu era acoperit
		// l-am adaugat primul pt ca restul il seteaza la 2 si s-ar fi executat de fiecare data
		// atentie!! va fi mult mai agresiva acum teoretic. Trebuie testat
		if (turnLeft == 2)
		{
			SETSPEED(speed+BOOST, speed+BOOST);
		}
		if (turnLeft==1)
		{
			SETSPEED(speed+BOOST, (int)((speed+BOOST)*1.2) );
			turnLeft = 2;
		}
		if (turnLeft ==0)
		{
			SETSPEED( (int)((speed+BOOST)*1.2), speed+BOOST);
			turnLeft = 2;
		}
	else 
	{
		if (turnLeft == 2)
		{
			SETSPEED(aux, aux);
		}
		if (turnLeft == 1)
		{
			SETSPEED(aux, (int)(aux*1.2) );//nr_osc = nr;
			turnLeft = 2;
		}
		if (turnLeft == 0)
		{
			SETSPEED( (int)(aux*1.2), aux);//nr_osc = nr;
			turnLeft = 2;
		}
		
	}
	if(ave/20<osc[19]-40 && lmc>=MCX)	//left break
	{
		if(aux>ACCELERATION)
		{
				
			FLAG_LEFT_BREAK = 1;
			TIME_COUNTER_BREAK = 0;
			LeftBreak(MAX_LEFT_BREAK);
			RightBreak(MAX_RIGHT_BREAK);
		}
			
	}	
	if(ave/20>osc[19]+40 && rmc>=MCX)	//right
	{
		if(aux>ACCELERATION)
		{
			FLAG_RIGHT_BREAK = 1;
			TIME_COUNTER_BREAK = 0;
			RightBreak(MAX_RIGHT_BREAK);
			LeftBreak(MAX_LEFT_BREAK);
		}
	}
}

void PrintSerial()
{
	int w = 0;
	if(counter == 2)
	{
		SETSPEED(0,0);
		for(w = 0;w<counter;w++)
			PrintResult(128*counter,128*(counter+1));
		counter = 0;
	}
}

/* fac validarea citirii curente pe baza celor anterioare: alea cele mai recente au ponderea cea mai 
 * mare in luarea deciziei
 * daca nu-i valida o neglijez
 */
int VALID(int centru)
{
	int y,sum =0;
	for(y = 0;y<3;y++)
	{
		sum+=angle_queue[y][0];
	}
	sum = sum + angle_queue[2][0]*2;
	valid_result = (int)(sum/5) + 1;
	if(centru>valid_result-30 && centru<valid_result+30)
		return 1;
	return 0;
	//return 1;
}


/* preiau date de la Fotorezistor si calculez intensitatea luminoasa a led-urilor */
void LightIntensity()
{
	
	uint8_t light;
	int q=0;
	//ADC.MCR.B.NSTART=1;     		/* Trigger normal conversions for ADC0 */
	//while (ADC.MCR.B.NSTART == 1) {};
	lightdata = ADC.CDR[4].B.CDATA;
	Delay();
	intensity = (uint8_t)(lightdata >> 2);
	//LED_intensity(1000-4*intensity);		
	LED_intensity(1000-4*intensity);		
}

void FullBreak(left_break,right_break)
{
		LeftBreak(left_break);
		EMIOS_0.CH[6].CBDR.R = 0;
		
		RightBreak(right_break);
		EMIOS_0.CH[7].CBDR.R = 0;
}

asm void initIrqVectors(void) {
  lis	   r3, __IVPR_VALUE@h   /* IVPR value is passed from link file */
  ori      r3, r3, __IVPR_VALUE@l 
  mtivpr   r3									 
}

void initINTC(void) {	
  INTC.MCR.B.HVEN = 0;       /* Single core: initialize for SW vector mode */
  INTC.MCR.B.VTES = 0;       /* Single core: Use default vector table 4B offsets */
  INTC.IACKR.R = (uint32_t) &IntcIsrVectorTable[0];    /* MPC555x: INTC ISR table base */
}

void initPIT(void) {
                            /* NOTE:  DIVIDER FROM SYSCLK TO PIT ASSUMES DEFAULT DIVIDE BY 1 */
  PIT.PITMCR.R = 0x00000001;       /* Enable PIT and configure timers to stop in debug mode */
  PIT.CH[1].LDVAL.R = 64000;       /* PIT1 timeout = 64000 sysclks x 1sec/64M sysclks = 1 msec */
  PIT.CH[1].TCTRL.R = 0x00000003; /* Enable PIT1 interrupt and make PIT active to count */ 
  INTC.PSR[60].R = 0x01;           /* PIT 1 interrupt vector with priority 1 */
}

void initINTC_PSR(void)
{
	INTC.PSR[4].R = 0x02;		/* Software interrupt 4 IRQ priority = 2 */
	INTC.PSR[41].R = 0x03;		/* External interrupt0 41 IRQ priority = 3 */
	INTC.PSR[42].R = 0x04;		/* External interrupt1 42 IRQ priority = 4 */	
	INTC.PSR[60].R = 0x01;      /* PIT 1 interrupt vector with priority 1 */	
}

void initSwIrq4(void) {
  INTC.PSR[4].R = 0x02;		/* Software interrupt 4 IRQ priority = 2 */
}

void initExtIrq0(void) {
  INTC.PSR[41].R = 0x03;		/* External interrupt0 41 IRQ priority = 3 */
}

void initExtIrq1(void) {
  INTC.PSR[42].R = 0x04;		/* External interrupt1 42 IRQ priority = 4 */
}

void initPitIrq(void) {
  INTC.PSR[60].R = 0x01;		/* PIT interrupt 4 IRQ priority = 1 */
}

void enableIrq(void) {
  INTC.CPR.B.PRI = 0;          /* Single Core: Lower INTC's current priority */
  asm(" wrteei 1");	    	   /* Enable external interrupts */
}

void initSIULIrqPads(void)
{
	SIU.IFCPR.R = 0x00000000;
	SIU.IRER.R = 0x00000401;
	SIU.IREER.R = 0x00000401;
	SIU.PCR[3].R = 0x0102;   
	SIU.PCR[74].R = 0x0102;    
}


//to do 
/*
	de verificat ca centrul este valid 
	!!! nu merge smecheria pe angle pt ca e influentat de pid si nu stii cum evolueaza treaba
	de facut un mecanism de anticipat umpic care va fi urmatoarea mutare, de predictie
*/

void main (void) {
	volatile int j = 0,flag=0; 			/* Dummy idle counter */
	int err,last_err,err_sum;
	int aux=0;
	char buffer[20];
	
	initModesAndClock(); 				/* Initialize mode entries and system clock */
	initPeriClkGen();  					/* Initialize peripheral clock generation for DSPIs */
	disableWatchdog(); 					/* Disable watchdog */
	init_LinFLEX_0_UART();
    initPads();             			/* Initialize pads used in example */
  	initADC();              			/* Init. ADC for normal conversions but don't start yet*/
  	initCTU();              			/* Configure desired CTU event(s) */
  	initEMIOS_0();          			/* Initialize eMIOS channels as counter, SAIC, OPWM */
  	initEMIOS_0ch3();					/* Initialize eMIOS 0 channel 3 as OPWM and channel 2 as SAIC*/ 
  	
  	initEMIOS_0ch0(); 					/* Initialize eMIOS 0 channel 0 as modulus counter*/
	initEMIOS_0ch23(); 					/* Initialize eMIOS 0 channel 23 as modulus counter*/
	initEMIOS_0ch4(); 					/* Initialize eMIOS 0 channel 0 as OPWM, ch 4 as time base */
	initEMIOS_0ch6(); 					/* Initialize eMIOS 0 channel 0 as OPWM, ch 6 as time base */
	initEMIOS_0ch7(); 					/* Initialize eMIOS 0 channel 1 as OPWM, ch 7 as time base */
	initEMIOS_0ch9();
	initEMIOS_0ch8();
	initEMIOS_0ch10();
	initEMIOS_0ch11();
	
	initIrqVectors();		/* Initialize exceptions: only need to load IVPR */
  	initINTC();			/* Initialize INTC for software vector mode */
  	initPIT();
  	initSIULIrqPads();
  	initINTC_PSR();
	initSwIrq4();
	initExtIrq0();
	initExtIrq1();
	initPitIrq();
  	enableIrq();		   	/* Ensure INTC current prority=0 & enable IRQ */
	
	SIU.PCR[64].R = 0x0102;				/* Program the drive enable pin of S1 (PE0) as input*/
	SIU.PCR[65].R = 0x0102;				/* Program the drive enable pin of S2 (PE1) as input*/
	SIU.PCR[66].R = 0x0102;				/* Program the drive enable pin of S3 (PE2) as input*/
	SIU.PCR[67].R = 0x0102;				/* Program the drive enable pin of S4 (PE3) as input*/
	
	SIU.PCR[68].R = 0x0200;				/* Program the drive enable pin of LED1 (PE4) as output*/
  	SIU.PCR[69].R = 0x0200;				/* Program the drive enable pin of LED2 (PE5) as output*/
	SIU.PCR[70].R = 0x0200;				/* Program the drive enable pin of LED3 (PE6) as output*/
	SIU.PCR[71].R = 0x0200;				/* Program the drive enable pin of LED4 (PE7) as output*/
  	SIU.PGPDO[2].R |= 0x0f000000;		/* Disable LEDs*/
	
	SIU.PCR[17].R = 0x0200;				/* Program the drive enable pin of Right Motor as output*/
	SIU.PCR[16].R = 0x0200;				/* Program the drive enable pin of Left Motor as output*/
	SIU.PGPDO[0].R = 0x00000000;		/* Disable the motors */
	
	half_window_size = window_size/2;
	/* fac initializarea pt LIFO-ul cu deciziile:
	 * [x][0] tin minte centrul liniei negre(centrul pe care-l calculeaza)
	 * [x][1] decizia luata: unghiul calculat cu pid-ul si pe care-l dau servo-ului
	 */
	for(j=0 ;j<3;j++)
	{
		angle_queue[j][0] = 64;
		angle_queue[j][1] = STRAIGHT;		
	}
	//initializare pt vectorul de oscilatii
	for(j=0;j<20;j++)
		osc[j]=0;
	for(j=0;j<128;j++)
		Result_binar[j] = 0;
	ENABLEMOTORS();
	SETSPEED(speed,speed);
	CAMERA();
	WINDOW_NOISE_REDUCTION();
	GETMIJ(Result_binar);
	center = GETCENTER();
	//calculez eroarea pt pid(prima citire)
	err = 64 - center;
	//pun in lifo prima citire
	osc[19] = err;				
	last_err = err;
	err_sum = err;
	Amplitude();		
	j=0;	
	amount = 1000;
	/* Loop forever */
	for (;;) 
	{
		
		
		//LEFT_MOTOR_CURRENT();
		//RIGHT_MOTOR_CURRENT();
		//TransmitCharacter('r');
		//TransmitCharacter(' ');
		//TransmitNumber (rmc);
		//TransmitCharacter(' ');
		//TransmitCharacter('l');
		//TransmitCharacter(' ');
		//TransmitNumber (lmc);
		//TransmitCharacter('\n');
		//TransmitCharacter('\r');
		if(StartStopCounter>=2)	// numara cate linii de start/stop vede si face break
		{
			FullBreak(MAX_LEFT_BREAK,MAX_RIGHT_BREAK);
			break;	
		}
		LightIntensity();
		ADJUSTMENTS();
		CAMERA();
		
		WINDOW_NOISE_REDUCTION();
		
		GETMIJ(Result_binar);
		center = GETCENTER();
		if(center!= -1)
		{
			err = 64-center;
			for(j = 0;j<19;j++)
			{
				osc[j] = osc[j+1];
			}
				
			osc[19] = err;				
			
			err_sum +=err;
			if(err_sum>500)
				err_sum = 500;
			if(err_sum<-500)
				err_sum = -500;
			/* calculez pid-ul-> sa-mi zici daca-i bine
			 * e normalizat... ala nenormalizat de mai jos da cu virgula
			 * crapa ceva numeric la el.Nu stiu ce dar la un moment dat ia o decizie
			 * pur si simplu aleatorie si iese de pe traseu( cu ala mi-a iesit la praga de pe traseu)
			 */
			viraj=(int)((380*(kp*err + ki*err_sum + kd*(err-last_err)))/(kp*50+ki*500+kd*10));
			amount = viraj;
			//viraj =(int)(kp*err + ki*err_sum + kd*(err-last_err));
			angle = STRAIGHT - viraj;
			if(angle>MAX_RIGHT)
				angle=MAX_RIGHT;
			if(angle<MAX_LEFT)
				angle=MAX_LEFT;
			/* am lasat o gaura la mijloc de 20 de pixeli pentru a da 
			* boost pe ambele motoare pentru oscilatii mici
			*/
			if (center < 54)
				turnLeft = 1;
			else
				if (center > 74)
					turnLeft = 0;
				else
					turnLeft = 2;
			/*
			 * daca validarea citirii reuseste, adaug citirea si comanda in coada
			 */
			if(VALID(center)==1)
			{
				Deplasare_Stanga();
				angle_queue[2][0] = center;
				angle_queue[2][1] = angle;	
			}
			last_err = err;
			//calculez amplitudinile si nr oscilatiilor si stabilesc viteza
			Amplitude();		
			
		}
		//setez directia la servo cu ultima decizie luata
		TURNANGLE(angle_queue[2][1]);
		/* asta e frana cu servo-uri
		 * trebuie separata in 2 bucati:
		 * daca merge sub o anumita viteza sa franeze doar cu motoarele in scurt
		 * daca e pe full speed sa bage si servo-urile pe frana
		 */
		//SETSPEED(speed);*/
		
		
	}
}

//if ((Pit1Ctr & 1)==0) { /* If PIT1Ctr is even*/
  //  INTC.SSCIR[4].R = 2;      /*  then nvoke software interrupt 4 */
  //}
void Pit1ISR(void) {
  Pit1Ctr++;              /* Increment interrupt counter */
  if ((Pit1Ctr & 1)==0) { /* If PIT1Ctr is even*/
    INTC.SSCIR[4].R = 2;      /*  then nvoke software interrupt 4 */
  }
/* Use one of the folloiwng two lines: */
/*PIT.PITFLG.B.TIF1 = 1; */    /* MPC551x, MPC563xM: Clear PIT 1 flag by writing 1 */
  PIT.CH[1].TFLG.B.TIF = 1;    /* MPC56xxB/P/S: Clear PIT 1 flag by writing 1 */
}

void SwIrq4ISR(void) {
	SWirq4Ctr++;
	if (FLAG_START_STOP==1)
	{
		TIME_COUNTER_IRQ++;
		if (TIME_COUNTER_IRQ>=500)
		{
			FLAG_START_STOP = 0;
			TIME_COUNTER_IRQ = 0;
		}
	}
	if(FLAG_LEFT_BREAK==1)
	{
		TIME_COUNTER_BREAK++;
		if(TIME_COUNTER_BREAK==BREAKING_TIME/2)
		{
			RightBreak(MIN_RIGHT_BREAK);
		}
		if(TIME_COUNTER_BREAK>=BREAKING_TIME)
		{
			LeftBreak(MIN_LEFT_BREAK);
			TIME_COUNTER_BREAK = 0;
			FLAG_LEFT_BREAK = 0;
		}
	}
	if(FLAG_RIGHT_BREAK == 1)
	{
		TIME_COUNTER_BREAK++;
		if(TIME_COUNTER_BREAK==BREAKING_TIME/2)
		{
			LeftBreak(MIN_LEFT_BREAK);
		}
		if(TIME_COUNTER_BREAK>=BREAKING_TIME)
		{
			RightBreak(MIN_RIGHT_BREAK);
			TIME_COUNTER_BREAK = 0;
			FLAG_RIGHT_BREAK = 0;
		}
	}
	SIU.PGPDO[2].R &= 0x0e000000;		/* Enable LED4*/
	for(SWirq4Ctr = 0;SWirq4Ctr<10;SWirq4Ctr++)
	{
		
	}
	SIU.PGPDO[2].R |= 0x01000000;		/* Disable LED4*/
  	INTC.SSCIR[4].R = 1;		/* Clear channel's flag */  
}
//intrerupere pe PA3
void ExtIrq0(void)
{
	if (FLAG_START_STOP != 1)
	{
		StartStopCounter ++;
		TIME_COUNTER_IRQ = 0;
		FLAG_START_STOP = 1;	
		SIU.PGPDO[2].R &= 0x07000000;		/* Enable LED1*/
		for(SWirq4Ctr = 0;SWirq4Ctr<500;SWirq4Ctr++)
		{
		
		}
		SIU.PGPDO[2].R |= 0x08000000;		/* Disable LED1*/
	}
	SIU.ISR.R = 0x00FF;
}
//intrerupere pe PE10
void ExtIrq1(void)
{
	SIU.PGPDO[2].R &= 0x0b000000;		/* Enable LED2*/
	for(SWirq4Ctr = 0;SWirq4Ctr<500;SWirq4Ctr++)
	{
		
	}
	SIU.PGPDO[2].R |= 0x04000000;		/* Disable LED2*/
	SIU.ISR.R = 0xFF00;
}